﻿#include "StdAfx.h"
#include "CgroundClassify.h"


#pragma region 调用 CpointsAnalyze 类的变量

extern cv::Mat g_imgGround;
extern bool g_bRecieve_New_Ground_Image;

#pragma endregion 调用 CpointsAnalyze 类的变量



/*----------------------------
 * 功能 : 类实例初始化，设置内部变量初始值
 *----------------------------
 * 函数 : CgroundClassify::CgroundClassify
 * 访问 : public 
 * 返回 : 
 *
 * 参数 : void
 */
CgroundClassify::CgroundClassify(void)
{
}


/*----------------------------
 * 功能 : 类析构函数，等待循环线程返回
 *----------------------------
 * 函数 : CgroundClassify::~CgroundClassify
 * 访问 : virtual public 
 * 返回 : 
 *
 * 参数 : void
 */
CgroundClassify::~CgroundClassify(void)
{
	if (m_iInitResult == 0)
	{
		m_bSaveTrainImage = false;
		m_bExit = true;
		gcThread.join();
	}
}


/*----------------------------
 * 功能 : 类实例初始化，检查保存训练和测试图像的文件夹，并进入循环线程
 *			测试模式下，加载训练图像特征库、初始化Matlab DLL文件 和 SVM分类器（可选）
 *			训练模式下，直接进入循环线程
 *----------------------------
 * 函数 : CgroundClassify::start
 * 访问 : public 
 * 返回 : int		初始化成功 返回 0，否则 返回 -1
 */
int 
CgroundClassify::start()
{
	m_iGroundType = -1;

	// 检查图像训练库和测试库的文件夹路径
	initCheckDirs();

	// 如果运行 测试 模式，则首先读入训练图像的特征库
	m_bInTestMode = CparamServer::instance()->InGroundTestMode();
	if (m_bInTestMode)
	{
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 运行 测试 模式 ！ ") );
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 载入地面图像样本特征库 ...") );

		loadTrainData(m_matTrainData, m_strTrainPath);
		if (m_matTrainData.empty())
		{
			MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 载入特征库失败！") );
			m_iInitResult = -1;
			return -1;
		}
		
#ifdef _USE_SVM_
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 正在创建并训练 SVM 分类器 ...") );
		trainSVM(m_matTrainData, m_SVM);
#endif

	}
	else
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 运行 训练 模式 ！") );

	// 进入循环线程
	gcThread = boost::thread(
		boost::bind(&CgroundClassify::gcRun, this));

	m_iInitResult = 0;
	return 0;
}


/*----------------------------
 * 功能 : 获取地面类型
 *----------------------------
 * 函数 : CgroundClassify::getGroundType
 * 访问 : public 
 * 返回 : string		地面类型字符串
 */
string 
CgroundClassify::getGroundType()
{
	string strGroundType = "未知";

	if (gcMutex.try_lock())
	{
		if (m_iGroundType >= 0 && m_iGroundType < m_labelNames.size())
		{
			strGroundType = m_labelNames[m_iGroundType];
		}

		gcMutex.unlock();
	}

	return strGroundType;
}


/*
 *------------------------------------------
 *	            内部函数
 *------------------------------------------
 */


/*----------------------------
 * 功能 : 地面类型判断主线程
 *			训练模式，主要完成 采集训练图像 并 保存至指定的本地文件夹 的功能
 *			测试模式，主要完成 采集保存测试图像 并 判断地面类型 的功能
 *----------------------------
 * 函数 : CgroundClassify::gcRun
 * 访问 : private 
 * 返回 : void
 */
void 
CgroundClassify::gcRun()
{
	m_bExit = false;
	m_bSaveTrainImage = false;

	if ( m_bInTestMode )	// 运行 测试 模式
	{
		while ( !m_bExit )
		{
			if ( g_bRecieve_New_Ground_Image )		// 测试模式下自动判断地面类型并保存图像 
			{
				// 复制图片
				g_imgGround.copyTo( m_imgGround );

				// 判断地面类型
				vector<float> imgCode;
				imageEncoder( m_imgGround, imgCode );
				m_iGroundType = getClassifyResult( imgCode );

				WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 地面类型：" + getGroundType() );

				// 生成当前时间字符串，作为图像文件名
				string imgPath = m_strTestPath + "\\" + GET_TIME_STRING(FOR_FILE_NAME) + getGroundType() + ".jpg";

				// 保存图像
				imwrite(imgPath, m_imgGround);

				g_bRecieve_New_Ground_Image = false;
			} 
			else
			{
				Sleep(5);
			}
		}
	}
	else // 训练模式，只保存样本图像
	{
		clock_t preSaveTime = clock();
		clock_t	TIME_STEP = 1000;	// 每隔一秒保存一次图像

		while ( !m_bExit )
		{
			// 小于指定时间间隔时，静默循环
			if ( (clock() - preSaveTime) < TIME_STEP )
			{
				Sleep(5);
				continue;
			}

			if ( m_bSaveTrainImage && g_bRecieve_New_Ground_Image )	
			{
				// 复制图片
				g_imgGround.copyTo( m_imgGround );

				// 生成当前时间字符串，作为图像文件名
				string imgPath = m_strTrainPath + "\\" + GET_TIME_STRING(FOR_FILE_NAME) + ".jpg";
				
				// 保存图像
				imwrite(imgPath, m_imgGround);

				// 记录保存时间
				preSaveTime = clock();

				g_bRecieve_New_Ground_Image = false;
			} 
			else
			{
				Sleep(5);
			}
		}
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 退出 地面类型判断 循环线程 !") );
	}
}


/*----------------------------
 * 功能 : 检查图像训练和测试库的路径是否存在，若不存在则创建
 *----------------------------
 * 函数 : CgroundClassify::initCheckDirs
 * 访问 : private 
 * 返回 : void
 */
void 
CgroundClassify::initCheckDirs()
{
	// 设置训练图像库路径
	m_strTrainPath = CparamServer::instance()->GroundTrainPath();
	/* Check for existence */
	if( (CparamServer::instance()->CheckDir( m_strTrainPath.c_str(), false )) == -1 )
	{
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 路径不存在: " + m_strTrainPath ) );
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 正尝试创建 训练图像集 文件夹 !") );

		m_strTrainPath = "Images\\Train";
		if( (CparamServer::instance()->CheckDir( m_strTrainPath.c_str(), true )) == -1 )
		{
			MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 创建文件夹失败 !") );
			m_strTrainPath = "";
		}
	}

	// 设置测试图像库路径
	m_strTestPath = CparamServer::instance()->GroundTestPath();
	/* Check for existence */
	if( (CparamServer::instance()->CheckDir( m_strTestPath.c_str(), false )) == -1 )
	{
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 路径不存在!" + m_strTestPath ) );
		MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 正尝试创建 测试图像集 文件夹 !") );

		m_strTestPath = "Images\\Train";
		if( (CparamServer::instance()->CheckDir( m_strTestPath.c_str(), true )) == -1 )
		{
			MT_COUT( WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 创建文件夹失败 !") );
			m_strTestPath = "";
		}
	}
}


/*----------------------------
 * 功能 : 从指定路径读入训练数据
 *----------------------------
 * 函数 : CgroundClassify::loadTrainData
 * 访问 : private 
 * 返回 : void
 *
 * 参数 : cv::Mat &	data	[out]	训练数据矩阵
 * 参数 : string		dbPath	[in]	指定文件路径
 */
void
CgroundClassify::loadTrainData(cv::Mat& data, string dbPath)
{
	// 打开文件
	string dictPath = dbPath + "\\" + "traindata.txt";
	string labelPath = dbPath + "\\" + "labelname.txt";

	ifstream dictFile(dictPath.c_str());
	ifstream labelFile(labelPath.c_str());
	if(!dictFile || !labelFile)
	{
		cout<<WRITE_LOG(LOG_LEVEL_SUB_1, "[- Ground Classify] 文件打开错误: " + dictPath ) <<endl; 
		return;
	}

	// 读入数据
	istream_iterator<float> begin(dictFile);
	istream_iterator<float> end;
	vector<float> inData(begin,end);

	// 转换为矩阵形式
	int vectorLength = inData[0];
	int sampleCount = inData.size() / vectorLength;
	cv::Mat matTrainData = cv::Mat(inData).reshape(1, sampleCount).clone();

	// 提取有效数据
	data = matTrainData.rowRange(1,sampleCount).clone();	//第一行（行号为0）是无效的全零序列，故取第二行至最后行的数据

	// 提取分类标签名称
	m_labelNames.push_back("Unknown");
	string line;
	while ( getline(labelFile, line) )
	{
		m_labelNames.push_back(line);
	}

	labelFile.close();
	dictFile.close (); 

}

/*----------------------------
 * 功能 : 计算归一化颜色直方图
 *----------------------------
 * 函数 : ComputeNormalizedColorHist
 * 访问 : public 
 * 返回 : void
 *
 * 参数 : cv::Mat &			img		[in]	输入图像，必须是彩色图像，否则返回空序列
 * 参数 : int				N		[in]	单通道直方图长度
 * 参数 : vector<float> &	hist	[out]	颜色直方图序列
 */
void 
CgroundClassify::ComputeNormalizedColorHist(cv::Mat& img, int N, vector<float>& hist)
{
	/***
	 *	提取 灰度矩阵 或 色度矩阵
	 */
	cv::Mat h;
	int C = img.channels();	//图像通道数
	if (C == 3)
	{
		cvtColor(img, h, CV_RGB2HSV);
	}
	else
	{
		hist.clear();
		return;
	}

	/***
	 *	计算直方图
	 */
	int M = img.rows * img.cols;	// 图像像素总数
	float S = 1.0f / M;				// 直方图归一化元值
	cv::MatConstIterator_<cv::Vec3b>	
		it = h.begin<cv::Vec3b>(),		// 矩阵指针
		it_end = h.end<cv::Vec3b>();

	hist.resize(N*C,0);	// 初始化直方图序列长度
	for (;it != it_end; ++it)
	{
		cv::Vec3b pix = *it;			//像素值
		UINT id0 = pix[0] * N / 256,	//在直方图序列的位置
			 id1 = pix[1] * N / 256 + N,
			 id2 = pix[2] * N / 256 + N + N;
		hist[id0] += S;				//对应位置的值增加一个元值
		hist[id1] += S;				//对应位置的值增加一个元值
		hist[id2] += S;				//对应位置的值增加一个元值
	}
}


/*----------------------------
 * 功能 : 生成图像特征编码
 *----------------------------
 * 函数 : CgroundClassify::imageEncoder
 * 访问 : private 
 * 返回 : 
 * 
 * 参数 : cv::Mat &			img			[in]	图像
 * 参数 : vector<float>&		vecCode		[out]	图像编码
 */
void 
CgroundClassify::imageEncoder(cv::Mat& img, vector<float>& vecCode)
{
	vector<double> vecCEDD;
	vector<double> vecFCTH;
	vector<double> vecJCD;

	CEDD cedd;
	FCTH fcth;
	JCD jcd;

	// 计算图像编码
	vecCEDD = cedd.Apply(img);
	vecFCTH = fcth.Apply(img, 2);
	vecJCD = jcd.JointHistograms(vecCEDD, vecFCTH);

	// double >> float
	vecCode.resize(vecJCD.size());
	for (size_t i=0;i<vecJCD.size();i++)
	{
		vecCode[i] = vecJCD[i];
	}

	//// 补充颜色直方图特征序列
	//int N2 = 12;
	//vector<float> hist;
	//computeNormalizedColorHist(*img, N2, hist);

	//// 合并特征序列
	//vector<float> vecCode(lenJCD+N2*3,0);
	//copy( vecJCD.begin(), vecJCD.end(), vecCode.begin() );
	//copy( hist.begin(), hist.end(), vecCode.begin()+lenJCD );
}


/*----------------------------
 * 功能 : 获取图像分类结果
 *----------------------------
 * 函数 : CgroundClassify::getClassifyResult_2
 * 访问 : private 
 * 返回 : int							[ret]	图像分类序号
 * 
 * 参数 : vector<float> & imgFeature		[in]	图像特征序列
 */
int 
CgroundClassify::getClassifyResult(vector<float>& imgFeature)
{
	float iFlag = 0;

#ifdef _USE_SVM_
	iFlag = m_SVM.predict( cv::Mat(imgFeature) );	// 使用 SVM 分类器
#else
	iFlag = TanimotoClassifer(imgFeature);	// 使用 Tanimoto 距离度量分类
#endif

	return (int)iFlag;
}


#ifdef _USE_SVM_
/*----------------------------
 * 功能 : 建立并训练 SVM 分类器
 *----------------------------
 * 函数 : CgroundClassify::trainSVM
 * 访问 : private 
 * 返回 : void
 *
 * 参数 : cv::Mat &	data		[in]	训练数据
 * 参数 : cv::SVM &	svmModel	[out]	SVM 分类器
 */
void 
CgroundClassify::trainSVM(cv::Mat& data, cv::SVM& svmModel)
{
	cv::Mat matData = data.colRange(1, data.cols);
	cv::Mat matLabel0 = data.col(0);
	cv::Mat matLabel = cv::Mat::zeros(matLabel0.rows,1,CV_32FC1);
	for (int t=0;t<matLabel0.rows;t++)
	{
		matLabel.at<float>(t,0) = (int)(matLabel0.at<float>(t,0));
	}

	CvSVMParams param;

	param.svm_type = 100;
	param.kernel_type = 1;
	param.degree = 4;
	param.gamma = 4;
	param.coef0 = 1;

	svmModel.train(matData, matLabel, cv::Mat(), cv::Mat(), param);	
}

#else

/*----------------------------
 * 功能 : Tanimoto 相似度计算
 * 参考 : http://en.wikipedia.org/wiki/Jaccard_index
 *----------------------------
 * 函数 : TanimotoSimilarity
 * 访问 : private 
 * 返回 : float				[ret]	相似度值，0 - 100
 * 参数 : cv::Mat & obj		[in]	目标特征编码
 * 参数 : cv::Mat & train	[in]	训练数据特征编码
 */
float 
CgroundClassify::TanimotoSimilarity(cv::Mat& obj, cv::Mat& train)
{
	float result = 0;
	float sum1 = 0, sum2 = 0;
	float t11 = 0, t22 = 0, t12 = 0;

	// 求和
	cv::Scalar s1 = sum(obj), s2 = sum(train);
	sum1 = s1[0];
	sum2 = s2[0];

	// 计算相似度
	if (sum1 == 0 || sum2 == 0) result = 0;
	if (sum1 > 0 && sum2 > 0)
	{
		for (int i=0;i<obj.cols;i++)
		{
			float ov = obj.at<float>(0,i) / sum1;
			float tv = train.at<float>(0,i) / sum2;
			t11 += ov * ov;
			t22 += tv * tv;
			t12 += ov * tv;
		}

		result = 100 * ( t12 / ( t11 + t22 - t12 ) );
	}

	return result;
}


/*----------------------------
 * 功能 : 基于Tanimoto距离度量的图像分类器
 *----------------------------
 * 函数 : CgroundClassify::TanimotoClassifer
 * 访问 : private 
 * 返回 : int					[ret]	图像类型，-1 表示分类失败，0 表示未知类别，>0 为类别序号
 * 参数 : vector<float> & obj	[in]	目标图像特征编码
 */
int 
CgroundClassify::TanimotoClassifer(vector<float>& obj)
{
	cv::Mat traindata = m_matTrainData.colRange(1, m_matTrainData.cols);	//训练样本数据
	cv::Mat label = m_matTrainData.col(0);	//样本分类标签

	if ( (obj.size() != traindata.cols))
		return -1;

	int dataCount = traindata.rows;
	float similarity = 0.0f, maxSimilarity = 0.0f;
	int matchLabel = 0;	//匹配类别
	int matchIndex = -1;
	int THRESH_SIMILARITY_PRE = 85;
	int THRESH_SIMILARITY_UPDATE = 50;

	cv::Mat matObj = cv::Mat(obj);
	if (matObj.rows > 1)
		matObj = matObj.t();

	// 计算与全体样本的相似度，更新最相似样本的编码序列
	for (int i=0;i<dataCount;i++)
	{
		similarity = TanimotoSimilarity(matObj, traindata.row(i));
		if (maxSimilarity < similarity)
		{
			maxSimilarity = similarity;
			matchIndex = i;
		}
	}
	if (maxSimilarity > THRESH_SIMILARITY_UPDATE)
	{
		matchLabel = cvRound( label.at<float>(matchIndex,0) );
	}
	else
		matchLabel = 0;

	/***
	 *	TODO: 采用概率方法，降低误判率
	 */

	return matchLabel;
}


#endif

